package com.yx.cloud.utils;

import com.iot.product.sweeper.bean.PathPoint;

import java.util.ArrayList;

//
//import static java.lang.Math.PI;
//import static java.lang.Math.acos;
//import static java.lang.Math.pow;
//import static java.lang.Math.sqrt;
//
public class PathUtils {
    //    //linear_fit0(path_xx, path_yy, x, y, len);//排除异常点
//    //linear_fit1(path_xx, path_yy, x, y, len);//对拐角点进行处理
//    //linear_fit2(path_xx, path_yy, x, y, len, 2);//二次处理排除异常点
//
//    public static void linear_fit0(int x_pre[], int y_pre[], int x[], int y[], int len) {
//
//        if(len < 3) {
//            for(int i = 0; i < len; ++i) {
//                x[i] = x_pre[i];
//                y[i] = y_pre[i];
//            }
//        }
//
//        else {
//            x[0] = x_pre[0]; y[0] = y_pre[0];
//            x[1] = x_pre[1]; y[1] = y_pre[1];
//            x[len-2] = x_pre[len-2]; y[len-2] = y_pre[len-2];
//            x[len-1] = x_pre[len-1]; y[len-1] = y_pre[len-1];
//
//            for(int i = 2; i < len-2; ++i) {
//
//                if((x_pre[i-1] != x_pre[i] && x_pre[i-1] == x_pre[i+1] && (x_pre[i-2] == x_pre[i-1] || x_pre[i+2] == x_pre[i+1])) ||
//                        (y_pre[i-1] != y_pre[i] && y_pre[i-1] == y_pre[i+1] && (y_pre[i-2] == y_pre[i-1] || y_pre[i+2] == y_pre[i+1]))) {
//                    x[i] = (x_pre[i-1] + x_pre[i+1])/2;
//                    y[i] = (y_pre[i-1] + y_pre[i+1])/2;
//                }
//
//                else {
//                    x[i] = x_pre[i]; y[i] = y_pre[i];
//                }
//            }
//        }
//
//        for(int i = 0; i < len; i ++) {
//            x_pre[i] = x[i]; y_pre[i] = y[i];
//        }
//    }
//
//    public static void linear_fit1(int x_pre[], int y_pre[], int x[], int y[], int len) {
//
//        if(len < 3) {
//            for(int i = 0; i < len; ++i) {
//                x[i] = x_pre[i];
//                y[i] = y_pre[i];
//            }
//        }
//
//        else {
//            x[0] = x_pre[0]; y[0] = y_pre[0];
//            x[1] = x_pre[1]; y[1] = y_pre[1];
//            x[len-2] = x_pre[len-2]; y[len-2] = y_pre[len-2];
//            x[len-1] = x_pre[len-1]; y[len-1] = y_pre[len-1];
//
//            for(int i = 2; i < len-2; ++i) {
//
//                if(x_pre[i-2] == x_pre[i-1] && x_pre[i-1] == x_pre[i] && (x_pre[i+1] > x_pre[i] && x_pre[i+1] < x_pre[i+2]) && (y_pre[i+1] >= y_pre[i] && y_pre[i+1] >= y_pre[i+2])) {
//                    x[i] = x_pre[i];
//                    y[i] = y_pre[i+1];
//                }
//
//                else if(x_pre[i+2] == x_pre[i+1] && x_pre[i+1] == x_pre[i] && (x_pre[i-1] < x_pre[i] && x_pre[i-1] > x_pre[i-2]) && (y_pre[i-1] >= y_pre[i] && y_pre[i-1] >= y_pre[i-2])) {
//                    x[i] = x_pre[i];
//                    y[i] = y_pre[i-1];
//                }
//
//                else if(x_pre[i-2] == x_pre[i-1] && x_pre[i-1] == x_pre[i] && (x_pre[i+1] > x_pre[i] && x_pre[i+1] < x_pre[i+2]) && (y_pre[i+1] <= y_pre[i] && y_pre[i+1] <= y_pre[i+2])) {
//                    x[i] = x_pre[i];
//                    y[i] = y_pre[i+1];
//                }
//
//                else if(x_pre[i+2] == x_pre[i+1] && x_pre[i+1] == x_pre[i] && (x_pre[i-1] < x_pre[i] && x_pre[i-1] > x_pre[i-2]) && (y_pre[i-1] <= y_pre[i] && y_pre[i-1] <= y_pre[i-2])) {
//                    x[i] = x_pre[i];
//                    y[i] = y_pre[i-1];
//                }
//
//                else if(y_pre[i-2] == y_pre[i-1] && y_pre[i-1] == y_pre[i] && (y_pre[i+1] > y_pre[i] && y_pre[i+1] < y_pre[i+2]) && (x_pre[i+1] >= x_pre[i] && x_pre[i+1] >= x_pre[i+2])) {
//                    x[i] = x_pre[i+1];
//                    y[i] = y_pre[i];
//                }
//
//                else if(y_pre[i+2] == y_pre[i+1] && y_pre[i+1] == y_pre[i] && (y_pre[i-1] < y_pre[i] && y_pre[i-1] > y_pre[i-2]) && (x_pre[i-1] >= x_pre[i] && x_pre[i-1] >= x_pre[i-2])) {
//                    x[i] = x_pre[i-1];
//                    y[i] = y_pre[i];
//                }
//
//                else if(y_pre[i-2] == y_pre[i-1] && y_pre[i-1] == y_pre[i] && (y_pre[i+1] > y_pre[i] && y_pre[i+1] < y_pre[i+2]) && (x_pre[i+1] <= x_pre[i] && x_pre[i+1] <= x_pre[i+2])) {
//                    x[i] = x_pre[i+1];
//                    y[i] = y_pre[i];
//                }
//
//                else if(y_pre[i+2] == y_pre[i+1] && y_pre[i+1] == y_pre[i] && (y_pre[i-1] < y_pre[i] && y_pre[i-1] > y_pre[i-2]) && (x_pre[i-1] <= x_pre[i] && x_pre[i-1] <= x_pre[i-2])) {
//                    x[i] = x_pre[i-1];
//                    y[i] = y_pre[i];
//                }
//
//                else {
//                    x[i] = x_pre[i]; y[i] = y_pre[i];
//                }
//
//            }
//        }
//
//        for(int i = 0; i < len; i ++) {
//            x_pre[i] = x[i]; y_pre[i] = y[i];
//        }
//    }
//
//    public static void linear_fit(int x_pre[], int y_pre[], int x[], int y[], int len) {
//
//        if(len < 3) {
//            for(int i = 0; i < len; ++i) {
//                x[i] = x_pre[i];
//                y[i] = y_pre[i];
//            }
//        }
//        else {
//            x[0] = x_pre[0]; y[0] = y_pre[0];
//            x[1] = (x_pre[0] + x_pre[1] + x_pre[2])/3;
//            y[1] = (y_pre[0] + y_pre[1] + y_pre[2])/3;
//            x[len-2] = (x_pre[len-3] + x_pre[len-2] + x_pre[len-1])/3;
//            y[len-2] = (y_pre[len-3] + y_pre[len-2] + y_pre[len-1])/3;
//            x[len-1] = x_pre[len-1]; y[len-1] = y_pre[len-1];
//
//            for(int i = 2; i < len-2; ++i) {
//
//                x[i] = (x_pre[i-1] + x_pre[i] + x_pre[i+1])/3;
//                y[i] = (y_pre[i-1] + y_pre[i] + y_pre[i+1])/3;
//
//            }
//        }
//
//        curve_out(x, y, len, 4, 175);
//
//        int k, ie = 1, je = 1, istart = 1, iend = 1;
//        while(ie < len-1) {
//            if(angle(x[ie-1], y[ie-1], x[ie], y[ie], x[ie+1], y[ie+1]) > PI/180*175) {
//
//                je = ie+1;
//                if(je < len-1) {
//                    istart = ie;
//                    k = 0;
//                    while(je < len-1 && angle(x[je-1], y[je-1], x[je], y[je], x[je+1], y[je+1]) > PI/180*175) {
//                        k ++; je ++;
//                    }
//                    iend = je;
//                    for(int cc = istart+1; cc <= istart+k; cc ++) {
//                        x[cc] = (x[cc-1] + x[cc] + x[cc+1])/3;
//                        y[cc] = (y[cc-1] + y[cc] + y[cc+1])/3;
//                    }
//                    ie = iend;
//                }
//                else {break;}
//            }
//            else {ie ++;}
//        }
//
//    }
//
//    public static void linear_fit2(int x_pre[], int y_pre[], int x[], int y[], int len, int n) {
//
//        for(int i = 1; i <= n; i ++) {
//            linear_fit(x_pre, y_pre, x, y, len);
//
//            for(i = 0; i < len; i ++) {
//                x_pre[i] = x[i]; y_pre[i] = y[i];
//            }
//        }
//    }
//
//   public static double angle(int x1, int y1, int x2, int y2, int x3, int y3) {
//        double aa = pow(x1 - x2, 2) + pow(y1 - y2, 2);
//        double bb = pow(x3 - x2, 2) + pow(y3 - y2, 2);
//        double cc = pow(x3 - x1, 2) + pow(y3 - y1, 2);
//        double C = acos((aa + bb - cc)/(2 * sqrt(aa) * sqrt(bb)));
//        //  cout<<aa<<endl;
//        //   cout<<bb<<endl;
//        //  cout<<cc<<endl;
//        //  cout<<cc<<endl;
//        return C;
//    }
//
//    public static void curve_out(int x[], int y[], int len, int l, int C) {
//
//        for(int i = 0; i < len-l; i +=l) {
//            if(angle(x[i], y[i], x[i+l/2], y[i+l/2], x[i+l], y[i+l]) > PI/180*C) {
//                for(int j = i+1; j < i+l; j ++) {
//                    x[j] = (x[j-1] + x[j] + x[j+1])/3;
//                    y[j] = (y[j-1] + y[j] + y[j+1])/3;
//                }
//            }
//        }
//    }
    public static float getRotationFromPath(ArrayList<PathPoint> pathPoints) {
        if (pathPoints == null || pathPoints.size() < 2) {
            return 0;
        }
        int size=pathPoints.size();
        PathPoint lastPathPoint1=pathPoints.get(size-1);
        PathPoint lastPathPoint2=pathPoints.get(size-2);
        double radiu = Math.atan2((lastPathPoint1.y - lastPathPoint2.y), (lastPathPoint1.x - lastPathPoint2.x)); //弧度
        double angle = radiu * (180 / Math.PI); //角度
        return (float) angle ;
    }
}
